#ifndef RANSACLINE_HPP
#define RANSACLINE_HPP

#include <opencv2/opencv.hpp>
#include "ransac.hpp"
#if 0
#include <Eigen/Dense>
#include <unsupported/Eigen/Polynomials>
#endif
class RansacLine : public Ransac<cv::Vec2f,cv::Vec3f>
{

public:
    void Setup(const std::vector<cv::Vec2f> &pts, double ap, double et)
    {
        Release();

        nData=pts.size();
        nSelect=2;
        data=new cv::Vec2f[nData];

        for(size_t i=0;i<nData;i++)
        {
            data[i][0]=pts[i][0];
            data[i][1]=pts[i][1];
        }

        acceptPercentage=ap;
        errorThreshold=et;
    }


    void Setup(const cv::Vec2f *pts, size_t npt, double ap, double et)
    {
        Release();

        nData=npt;
        nSelect=2;
        data=new cv::Vec2f[nData];

        for(size_t i=0;i<nData;i++)
        {
            data[i][0]=pts[i][0];
            data[i][1]=pts[i][1];
        }

        acceptPercentage=ap;
        errorThreshold=et;
    }

    static cv::Vec3f ToHomogeneous(cv::Vec2f v)
    {
        return cv::Vec3f(v[0],v[1],1);
    }


    virtual void ComputeModel(const size_t *indexSelect, cv::Vec3f &modelFit) const
    {
        cv::Vec3f v[2];
        v[0]=ToHomogeneous(data[indexSelect[0]]);
        v[1]=ToHomogeneous(data[indexSelect[1]]);
        modelFit=v[0].cross(v[1]);
    }

    virtual void ComputeError(double *error, const cv::Vec3f &modelFit) const
    {
        double scale=1/sqrt(modelFit[0]*modelFit[0]+modelFit[1]*modelFit[1]);

        for(size_t i=0;i<nData;i++)
        {
            cv::Vec3f v=ToHomogeneous(data[i]);
            double errline=scale*(v.ddot(modelFit));
            error[i]=fabs(errline);
        }
    }

    static cv::Vec2d GetNormal(cv::Vec3f line)
    {
        double normalLength=fabs(line[2])/sqrt(line[0]*line[0]+line[1]*line[1]);

        double normalAngle=(line[2]<0?atan2(line[1],line[0]):atan2(-line[1],-line[0]));

        cv::Vec2d normal(normalLength,normalAngle);

        std::cout << "normal=" << normal <<"\t" << "line=" << line << "\n" << std::flush;

        return normal;
    }


#if 0

static std::vector<double> quarticSolver(double ce[5]) {
     //    std::cout<<"("<<ce[4]<<")x^4+("<<ce[3]<<")*x^3+("<<ce[2]<<")*x^2+("<<ce[1]<<")*x+("<<ce[0]<<")==0"<<std::endl;
//    double ce[5] = {  -0.0625, 0.5,  1.5000000000025215, 2.0000000000025215, 1};
     std::vector<double> ans(0, 0.);
     Eigen::PolynomialSolver<double, Eigen::Dynamic> solver;
     Eigen::VectorXd coeff(5);
     coeff[0] = ce[0];
     coeff[1] = ce[1];
     coeff[2] = ce[2];
     coeff[3] = ce[3];
     coeff[4] = ce[4];
     solver.compute(coeff);
     solver.realRoots(ans);
     return ans;
}

    void Refine()
    {
//        int *inflag=new int[nData];

        double scale=1/sqrt(model[0]*model[0]+model[1]*model[1]);

        double sxi2=0;
        double sxiyi=0;
        double syi2=0;
        double sxi=0;
        double syi=0;
        double s1i=0;
        double sxj=0;
        double syj=0;
        double s1j=0;

        for(size_t i=0;i<nData;i++)
        {
            cv::Vec3f v=ToHomogeneous(data[i]);
            double errline=scale*(v.ddot(model));

            double errparal=errline-distance;

            errline=fabs(errline);
            errparal=fabs(errparal);

            if(errline<errorThreshold)
            {
                sxi2+=data[i][0]*data[i][0];
                syi2+=data[i][1]*data[i][1];
                sxiyi+=data[i][0]*data[i][1];
                sxi+=data[i][0];
                syi+=data[i][1];
                s1i+=1;
//                inflag[i]=1;
            }
            else
                if(errparal<errorThreshold)
                {
                    sxi2+=data[i][0]*data[i][0];
                    syi2+=data[i][1]*data[i][1];
                    sxiyi+=data[i][0]*data[i][1];
                    sxi+=data[i][0];
                    syi+=data[i][1];
                    s1i+=1;
                    sxj+=data[i][0];
                    syj+=data[i][1];
                    s1j+=1;
//                    inflag[i]=2;
                }
//            else
//                    inflag[i]=0;
        }

        //line equation set to x*cos(theta)+y*sin(theta)+c=0
        //equation A*cos(theta)*sin(theta)+B*(sin^2(theta)-cos^2(theta))+C*cos(theta)+D*sin(theta)=0

        double A=s1i*(syi2-sxi2)+sxi*sxi-syi*syi;
        double B=sxi*syi-s1i*sxiyi;
        double C=distance*(s1j*syi-s1i*syj);
        double D=distance*(s1i*sxj-s1j*sxi);

        //let t=cos(theta)
        //above equation exapnd to a quartic equation c[4]*t^4+c[3]*t^3+c[2]*t^2+c[1]*t+c[0]=0

        double coef[5]={B*B-C*C, -2*A*C-2*B*D, -A*A-4*B*B+C*C+D*D, 2*A*C+4*B*D, A*A+4*B*B};

        std::vector<double> t=quarticSolver(coef);
//        std::cout<<"rts="<<cv::Mat(t)<<'\n'<<std::flush;
 //s=sin(theta),
        std::vector<double> s(t.size(),0);

        std::vector<double> c(t.size(),0);
        //error=sxi2*t^2+syi2*s^2+s1i*c^2+2*c*t*sxi+2*c*s*syi+2*s*t*sxiyi-2*distance*(t*sxj+s*syj+c*s1j)
        std::vector<double> error(t.size(),0);
        int minidx=-1;
        for(size_t i=0;i<t.size();i++)
        {
            if(t[i]>1 || t[i]<-1)
            {
                error[i]=-1;
                continue;
            }
            s[i]=-(B*(1-2*t[i]*t[i])+C*t[i])/(A*t[i]+D);

            c[i]=(distance*s1j-t[i]*sxi-s[i]*syi)/s1i;
            error[i]=sxi2*t[i]*t[i]
                    +syi2*s[i]*s[i]
                    +s1i*c[i]*c[i]
                    +2*c[i]*t[i]*sxi
                    +2*c[i]*s[i]*syi
                    +2*s[i]*t[i]*sxiyi
                    -2*distance*(t[i]*sxj+s[i]*syj+c[i]*s1j)
                    +s1j*distance*distance;

//            std::cout<<"root"<<i<<"="<<t[i]<<"\t"<<s[i]<<"\t"<<t[i]*t[i]+s[i]*s[i]<<"\t"<<error[i]/s1i<<"\n";

            if(minidx<0)
                minidx=i;
            else
                if(error[minidx]>error[i])
                    minidx=i;

        }
        if(minidx>=0)
//        std::cout<<"refine line="<<t[minidx]<<"x+"<<s[minidx]<<"y+"<<c[minidx]<<"=0\n"<<std::flush;
            GetNormal(cv::Vec3f(t[minidx],s[minidx],c[minidx]));

//        cv::Matx33d Am(sxi2,sxiyi,sxi,sxiyi,syi2,syi,sxi,syi,s1i);
//        cv::Vec3d modelRF=Am.inv()*(distance*cv::Vec3d(sxj,syj,s1j));

//        model[0]=modelRF[0];
//        model[1]=modelRF[1];
//        model[2]=modelRF[2];


//        GetNormal(CenterLine());
    }
#endif
};
#endif // RANSACPARALLELLINE_HPP
